#include "work.h"
typedef struct {
    float Kp, Ki, Kd;
    float lastError, integral;
} PID ;
void PID_Init(PID *pid, float Kp, float Ki, float Kd) {
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->lastError = 0;
    pid->integral = 0;
}
float PID_Update(PID *pid, float error) {
    pid->integral += error;
    float derivative = error - pid->lastError;
    float output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
    pid->lastError = error;
    return output;
}
